#include "rclcpp/rclcpp.hpp"
#include "interfaces/srv/set_goal.hpp"
#include <vector>
#include <string>

class TestSetGoalClient : public rclcpp::Node
{
public:
    TestSetGoalClient(int num_servers) : Node("test_set_goal_client"), num_servers_(num_servers)
    {
        // 动态创建服务客户端
        for (int i = 0; i < num_servers_; ++i)
        {
            std::string service_name = "/drone_" + std::to_string(i) + "/set_goal";
            auto client = this->create_client<interfaces::srv::SetGoal>(service_name);
            clients_.push_back(client);
        }
    }

    void send_request_to_all_servers()
    {
        for (int i = 0; i < num_servers_; ++i)
        {
            // 等待服务可用
            if (!clients_[i]->wait_for_service(std::chrono::seconds(10)))
            {
                RCLCPP_ERROR(this->get_logger(), "Service %s not available", clients_[i]->get_service_name());
                continue;
            }

            // 创建请求
            auto request = std::make_shared<interfaces::srv::SetGoal::Request>();
            request->goal_x = 250.0;
            request->goal_y = 250.0;
            request->goal_z = 15.0;

            // 异步发送请求并处理响应
            auto future = clients_[i]->async_send_request(request);

            // 等待结果
            if (rclcpp::spin_until_future_complete(shared_from_this(), future) == rclcpp::FutureReturnCode::SUCCESS &&
                future.get()->success)
            {
                RCLCPP_INFO(this->get_logger(), "Goal set successfully for %s", clients_[i]->get_service_name());
            }
            else
            {
                RCLCPP_ERROR(this->get_logger(), "Failed to set goal for %s", clients_[i]->get_service_name());
            }
        }
    }

private:
    int num_servers_;
    std::vector<rclcpp::Client<interfaces::srv::SetGoal>::SharedPtr> clients_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);

    // 可通过命令行参数或配置文件传递服务端数量
    int num_servers = 3; // 默认服务端数量
    if (argc > 1)
    {
        num_servers = std::stoi(argv[1]); // 第一个命令行参数作为服务端数量
    }

    auto client_node = std::make_shared<TestSetGoalClient>(num_servers);
    client_node->send_request_to_all_servers();

    rclcpp::shutdown();
    return 0;
}

